/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/*LINTLIBRARY*/
/*PROTOLIB1*/
/*#include <math.h>*/
#include "main.h"
#include "lsftable.h"
#include "code_lsf.h"
#include "move_array16.h"

int     BITS[10] = {3, 4, 4, 4, 4, 3, 3, 3, 3, 3};

#ifdef NEW_LSF_CODE
#define MIN_LSF_DIST    160     /* 39Hz * 32768/8000 scale factor */
#else
#define MIN_LSF_DIST    0
#endif

STATIC void AssignLSFFreqs(int findex[ORDER], fxpt_16 qlsp[ORDER]);

#if 0 /* function never called */
STATIC void CalcLSFFreq(
fxpt_16 lsp[ORDER],
int     frequency,
fxpt_16 lsf[ORDER]);
#endif

STATIC void CheckMono(
fxpt_16 table[LSFTABLE_LENGTH][LSFTABLE_WIDTH],
int     index[],
int     num,
fxpt_16 lsf[],
int     level,
fxpt_16 *errorup,
fxpt_16 *errordn,
int     *nonmono);

STATIC void CorrectNonMono(
fxpt_16 errorup,
fxpt_16 errordn,
int     findex[],
int     where,
int     level);

STATIC void InitLSFQuant(
fxpt_16 table[LSFTABLE_LENGTH][LSFTABLE_WIDTH],
fxpt_16 lsf[],
int     level,
int     lsfnum);

STATIC void QuantizeLSF(
fxpt_16 input,
fxpt_16 level[],
int     nlevel,
int     *findex);

/**************************************************************************
*                                                                         *
* ROUTINE
*               QuanLSF
*
* FUNCTION
*               Quantize line spectral frequencies
* SYNOPSIS
*               QuanLSF(lsf, qlsf, freq_index)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       lsf             fxpt_16  i      Unquantized LSFs
*       qlsf            fxpt_16  o      Quantized LSFs
*       freq_index      int      o      Indexes into frequency array
*
**************************************************************************/
void QuanLSF(
fxpt_16 lsf[ORDER],                     /* 0.15 format */
fxpt_16 qlsf[ORDER],                    /* 0.15 format */
int     freq_index[ORDER])
{
        fxpt_16 freqs[ORDER];           /* 0.15 format */
        int     i, level;
        fxpt_16 LSFInit[16];
        fxpt_16 errorup, errordn;
        int     nonmonotonic=FALSE;

        /* Calculate frequencies for LSPs */
        /* CalcLSFFreq(lsf, 8000, freqs); */
        MoveArray16(freqs, lsf, ORDER); /* lsf already in 0.15 format */

        /* For each LSF, calculate quantization level for ith LSF based
         * on bit allocation
         */
        for(i=0; i<ORDER; i++) {
                /*level = (int)pow(2.0, (double)BITS[i]) - 1;*/
                level = (1 << BITS[i]) - 1;

                /* Assign initial quantized LSFs */
                InitLSFQuant(LSFTable, LSFInit, level, i);

                /* Quantize LSFs */
                QuantizeLSF(freqs[i], LSFInit, level+1, &freq_index[i]);

                /* Check for monotonicity */
                if (i > 0)
                        CheckMono(LSFTable, freq_index, i, freqs, level,
                            &errorup, &errordn, &nonmonotonic);

                /* Correct if nonmonotonic */
                if (nonmonotonic)
                        CorrectNonMono(errorup, errordn, freq_index, i, level);
        }

        /* Quantize LSF frequencies using indices found above */
        AssignLSFFreqs(freq_index, qlsf);
}

/*************************************************************************
*                                                                        *
* ROUTINE
*               CalcLSFFreq
*
* FUNCTION
*               Calculate LSFs from LSPs using input frequency
* SYNOPSIS
*               CalcLSFFreq(lsp, frequency, lsf)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       lsp             fxpt_16   i     Frame of input line spectral pairs
*       frequency       int       i     Sampling frequency
*       lsf             fxpt_16   o     Frame of output line spectral frequencies
*
**************************************************************************/
#if 0 /* function never called */
void CalcLSFFreq(
fxpt_16 lsf[ORDER],             /* 0.15 format */
int     frequency,
fxpt_16 freqs[ORDER])           /* 15.0 format */
{
        int     i;

        /* Calculate LSF based on sampling frequency */
        for(i=0; i<ORDER; i++) {
                /*freqs[i] = (float)frequency * lsf[i];*/
                freqs[i] = fxpt_mult16_fast(frequency, lsf[i]);
        }
}
#endif

/*************************************************************************
*                                                                        *
* ROUTINE
*               InitLSFQuant
*
* FUNCTION
*               Initialize Quantized LSFs
* SYNOPSIS
*               InitLSFQuant(table, lsf, n)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       table           int       i     Table of LSFs
*       lsf             fxpt_16   o     Frame of output line spectral freqs
*       level           int       i     Number of levels
*       lsfnum          int       i     LSF number (not amount, but item num)
*
**************************************************************************/

void InitLSFQuant(
fxpt_16 table[LSFTABLE_LENGTH][LSFTABLE_WIDTH],
fxpt_16 lsf[],
int     level,
int     lsfnum)
{
        int     i;

        /* Assign values from table */
        for (i=0; i<=level; i++) {
                lsf[i] = table[lsfnum][i];
        }
}

/*************************************************************************
*                                                                        *
* ROUTINE
*               CheckMono
*
* FUNCTION
*               Check frequencies indices for monotonicity
* SYNOPSIS
*               CheckMono(table, index, num, lsf, errorup, errordn, nonmono)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       table           fxpt_16   i     Table of LSFs
*       index           int       i     Index into table
*       num             int       i     LSF number
*       lsf             fxpt_16   i     Line spectral frequencies
*       level           int       i     Quantization level
*       errorup         fxpt_16   o     Upward error correction
*       errordn         fxpt_16   o     Downward error correction
*       nonmono         int       o     Flag indicating lack of monotonicity
*
**************************************************************************/

void CheckMono(
fxpt_16 table[LSFTABLE_LENGTH][LSFTABLE_WIDTH],         /* 0.15 format */
int     index[],
int     num,
fxpt_16 lsf[],                  /* 0.15 format */
int     level,
fxpt_16 *errorup,               /* 0.15 format */
fxpt_16 *errordn,               /* 0.15 format */
int     *nonmono)
{
        /* Check this frequency against the previous one */
        if (table[num][index[num]]-MIN_LSF_DIST <= table[num-1][index[num-1]]) {
                *errorup = fxpt_add16(
                    fxpt_abs16(fxpt_sub16(lsf[num],
                        table[num][min(index[num]+1, level)])),
                    fxpt_abs16(fxpt_sub16(lsf[num-1],
                        table[num-1][index[num-1]])));

                *errordn = fxpt_add16(
                    fxpt_abs16(fxpt_sub16(lsf[num], table[num][index[num]])),
                    fxpt_abs16(fxpt_sub16(lsf[num-1],
                        table[num-1][max(index[num-1]-1, 0)])));

                *nonmono = TRUE;
        }
        else
                *nonmono = FALSE;
}


/*************************************************************************
*                                                                        *
* ROUTINE
*               CorrectNonMono
*
* FUNCTION
*               Correct index for minimum error if nonmonotonic
* SYNOPSIS
*               CorrectNonMono(errorup, errordn, findex, where, level)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       errorup         fxpt_16  i      Error adjustment measurement
*       errordn         fxpt_16  i      Error adjustment measurement
*       findex          int     i/o     Frequency index into LSFTable
*       where           int      i      Current LSF level
*       level           int      i      Number of quantization levels available
*
**************************************************************************/

void CorrectNonMono(
fxpt_16 errorup,                /* 0.15 format */
fxpt_16 errordn,                /* 0.15 format */
int     findex[],
int     where,
int     level)
{
        if (errorup < errordn) {
                findex[where] = min(findex[where]+1, level);

                while (LSFTable[where][findex[where]] - MIN_LSF_DIST <
                    LSFTable[where-1][findex[where-1]]) {
                        findex[where] = min(findex[where]+1, level);
                }
        }
        else {
                if (where == 1) {
                        findex[where-1] = max(findex[where-1]-1, 0);
                }
                else {
                        if (LSFTable[where-1][max(findex[where-1]-1,0)] -
                            MIN_LSF_DIST > LSFTable[where-2][findex[where-2]]) {
                                findex[where-1] = max(findex[where-1]-1, 0);
                        }
                        else {
                                findex[where] = min(findex[where]+1, level);

                                while(LSFTable[where][findex[where]] -
                                    MIN_LSF_DIST <
                                    LSFTable[where-1][findex[where-1]]) {
                                        findex[where] = min(findex[where]+1,
                                            level);
                                }
                        }
                }
        }
}


/**************************************************************************
*                                                                         *
* ROUTINE
*               AssignLSFFreqs
*
* FUNCTION
*               Correct index for minimum error if nonmonotonic
* SYNOPSIS
*               AssignLSFFreqs(findex, qlsp)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       findex          int      i      Frequency index into LSF Table
*       qlsp            fxpt_16  o      Quantized line spectral parameter
*
**************************************************************************/


void AssignLSFFreqs(
int     findex[ORDER],
fxpt_16 qlsp[ORDER])            /* 0.15 format */
{
        int     i;

        /* Quantize LSF frequencies using indices passed in */
        for(i=0; i<ORDER; i++) {
                qlsp[i] = LSFTable[i][findex[i]];
        }
}

/************************************************************************
*
* ROUTINE
*               UnquanLSF
*
* FUNCTION
*
*               Independent nonuniform scalar lsf decoder
*
* SYNOPSIS
*               UnquanLSF(findex,order,lsf)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       findex          int     i       lsf frequency index
*       no              int     i       lpc order
*       lsf             fxpt_16 o       lsf quantized frequency
*
***************************************************************************
*
* CALLED BY
*
*       codeparm
*
**************************************************************************/
void UnquanLSF(
int     findex[],
int     order,
fxpt_16 lsf[])
{
        int i;

        /* *** choose appropriate frequency by findex */

        for (i = 0; i < order; i++) {
                lsf[i] = LSFTable[i][findex[i]];
        }
}


/**************************************************************************
*
* ROUTINE
*               QuantizeLSF
*
* FUNCTION
*               Nearest output level scalar quantizer
*
* SYNOPSIS
*               QuantizeLSF(input, level, nlevel, findex)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       input           fxpt_16 i       Scalar to be quantized
*       level(nlevel)   int     i       Output level table
*       nlevel          int     i       Number of quantizer levels
*       findex          int     o       Index to quantized output (zero based!)
*
***************************************************************************/
void QuantizeLSF(
fxpt_16 input,                  /* 0.15 format */
fxpt_16 level[],
int     nlevel,
int     *findex)
{
        int     i;
        int     dist, low=0;

        /* Quantize to nearest output level: */

        low = abs(input - level[0]);
        *findex = 0;
        for (i=1; i<nlevel; i++) {
                dist = fxpt_abs16(input - level[i]);
                if (dist < low) {
                        low = dist;
                        *findex = i;
                }
        }
}
